#include "../include/px4control.h"
#include "../../../devel/include/Px4service/controlcmd.h"
#include "jsoncpp/json/json.h"
//目标位置坐标发布到tf-tree
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>

geometry_msgs::PoseStamped relative_target_pose;
geometry_msgs::PoseStamped target_pose;
geometry_msgs::TransformStamped transformStamped;

void tfbr(tf2_ros::TransformBroadcaster *br){
    // 填充TransformStamped消息
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "camera_odom_frame";       // 父坐标系
    transformStamped.child_frame_id = "target_pose_point";        // 子坐标系

    // 设置位移 
    transformStamped.transform.translation.x = target_pose.pose.position.x;
    transformStamped.transform.translation.y = target_pose.pose.position.y;
    transformStamped.transform.translation.z = target_pose.pose.position.z;

    // 设置旋转，使用四元数
    tf2::Quaternion q;
    q.setRPY(0, 0, 0); 
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    // 发布变换
    br->sendTransform(transformStamped);
}

class px4service : public px4control {
public:
    px4service(ros::NodeHandle *nh) : px4control(nh) {
    
    task_running = true;
    service = nh->advertiseService<Px4service::controlcmd::Request, Px4service::controlcmd::Response>("/Px4service/controlcmd",[this](Px4service::controlcmd::Request &req, Px4service::controlcmd::Response &res)->bool{
        return this->handle_string_request(req, res);
    });
    }

    bool handle_string_request(Px4service::controlcmd::Request &req,
                               Px4service::controlcmd::Response &res);

    bool task_running;
private:
    ros::ServiceServer service;
};


bool px4service::handle_string_request(Px4service::controlcmd::Request &req,
                           Px4service::controlcmd::Response &res) {
    if (req.modify == true)
    {
        target_pose.pose.position.x = req.p_x;
        target_pose.pose.position.y = req.p_y;
        target_pose.pose.position.z = req.p_z;
        this->task_running = req.run;
    }else{
        this->task_running = req.run;
    }
    
    res.p_x = target_pose.pose.position.x;
    res.p_y = target_pose.pose.position.y;
    res.p_z = target_pose.pose.position.z;
    res.run = this->task_running;
    ROS_INFO("------------------------------------------");
    ROS_INFO("Received request: %f, %f, %f, %d", res.p_x, res.p_y, res.p_z, res.run);
    ROS_INFO("Service state: %f, %f, %f, %d", target_pose.pose.position.x, target_pose.pose.position.y, target_pose.pose.position.z, this->task_running);
    return true;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "string_service_server");
    ros::NodeHandle n;
    px4service px4service(&n);
    tf2_ros::TransformBroadcaster br;
    ROS_INFO("Ready to process strings.");
    ros::Rate loop_rate(30);
    geometry_msgs::PoseStamped keep_pos;
    while (ros::ok())
    {
        // 若需要执行任务，则执行任务，否则保持当前位置
        if (px4service.task_running)
        {
            px4service.controlpos(target_pose);
        }else{
            keep_pos.pose.position.x = px4service.local_pos.pose.pose.position.x;
            keep_pos.pose.position.y = px4service.local_pos.pose.pose.position.y;
            keep_pos.pose.position.z = px4service.local_pos.pose.pose.position.z;
            px4service.controlpos(keep_pos);
        }
        tfbr(&br);
        ros::spinOnce();
        loop_rate.sleep();
    }
    
    return 0;
}
